

#include "rviz_custom_plugins/panels/snapshot_plane_panel.h"
#include "rviz_custom_plugins/utils/interactive_marker_manager.h"
#include "rviz_custom_plugins/utils/interactive_marker_util.h"
#include <rviz_common/logging.hpp>
#include "rviz_custom_plugins/utils/interactive_marker_factory.h"
#include "rviz_custom_plugins/utils/project_manager.h"
#include "rviz_custom_plugins/models/planes.h"
#include "rviz_custom_plugins/utils/pcl_util.h"


using rviz_custom_plugins::utils::addMetadata;
using rviz_custom_plugins::utils::InteractiveMarkerManager;
using rviz_custom_plugins::utils::getMetadataItem;
using rviz_custom_plugins::utils::InteractiveMarkerFactory;
using rviz_custom_plugins::utils::pclPointToStr;
using rviz_custom_plugins::utils::strToPclPoint;
using rviz_custom_plugins::utils::ProjectManager;


using rviz_custom_plugins::utils::MARKER_ADDED;
using rviz_custom_plugins::utils::MARKER_UPDATED;
using rviz_custom_plugins::utils::MARKER_REMOVED;

using rviz_custom_plugins::models::PointNormal;
using rviz_custom_plugins::models::Plane;
using rviz_custom_plugins::models::Planes;


#include <QDir>
#include <QFile>
#include <QTextStream>
#include <nlohmann/json.hpp>

using nlohmann::json;


namespace rviz_custom_plugins
{

namespace panels
{


SnapshotPlanePanel::SnapshotPlanePanel(QWidget *parent) : rviz_common::Panel(parent), ui(new Ui::snapshotPlanePanel())
{
    ui->setupUi(this);
    ProjectManager* projectManager = ProjectManager::getInstance();
    treeModel = new PointGroupModel();
    treeModel->setGroupTitlePrefix("拍摄面");
    ui->treeView->setFocusPolicy(Qt::StrongFocus);
    ui->treeView->header()->hide();
    ui->treeView->setModel(treeModel);
    ui->treeView->setColumnWidth(0, 100);
    ui->treeView->setIndentation(15);
    // 设置后续列自动拉伸（从第1列开始到最后）
    ui->treeView->header()->setSectionResizeMode(1, QHeaderView::Stretch);
    ui->treeView->header()->setSectionResizeMode(2, QHeaderView::Stretch);
    ui->treeView->header()->setSectionResizeMode(3, QHeaderView::Stretch);

    mapNotifier = BagMapNotifier::getInstance();

    qRegisterMetaType<interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr>(
        "interactive_markers::InteractiveMarkerServer::FeedbackConstSharedPtr"
    );

    connect(treeModel, &PointGroupModel::itemOrderUpdated, this, &SnapshotPlanePanel::handlerMarkersReorder, Qt::DirectConnection);
    connect(ui->btnAddPlane, &QPushButton::clicked, this, &SnapshotPlanePanel::addPlaneButtonClicked);
    connect(ui->btnRemovePlane, &QPushButton::clicked, this, &SnapshotPlanePanel::removePlaneButtonClicked);
    connect(ui->btnRemovePoint, &QPushButton::clicked, this, &SnapshotPlanePanel::removePointButtonClicked);

    connect(mapNotifier, &BagMapNotifier::mapPointSelected, this, &SnapshotPlanePanel::handleMapPointSelected);

    connect(projectManager, &ProjectManager::projectCreated, this, &SnapshotPlanePanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectOpened, this, &SnapshotPlanePanel::handleProjectLoaded, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectSaved, this, &SnapshotPlanePanel::handleProjectSaved, Qt::DirectConnection);
    connect(projectManager, &ProjectManager::projectClosed, this, &SnapshotPlanePanel::handleProjectClosed, Qt::DirectConnection);

}



void SnapshotPlanePanel::load(const Config& config)
{
    Panel::load(config);
    // set marker server namespace
    QString nameSpace;
    config.mapGetString("NameSpace", &nameSpace);
    markerThread = InteractiveMarkerManager::getInstance()->addThread(nameSpace);
    connect(markerThread.get(), &InteractiveMarkerThread::interactiveMarkerFeedback, this, &SnapshotPlanePanel::handleMarkerFeedback, Qt::QueuedConnection);
    connect(markerThread.get(), &InteractiveMarkerThread::activeStatusChanged, this, &SnapshotPlanePanel::handleActiveStatusChanged, Qt::QueuedConnection);
    handleActiveStatusChanged(markerThread->getActive());
}


void SnapshotPlanePanel::handleMarkerFeedback(InteractiveMarkerServer::FeedbackConstSharedPtr feedback)
{
    // std::ostringstream sstm;
    // sstm << " feedback->marker_name:" << feedback->marker_name;
    // RVIZ_COMMON_LOG_INFO(sstm.str().c_str());

    // 处理新增marker
    if(MARKER_ADDED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "add marker marker_name:" << feedback->marker_name;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
        return;
    }
    else if(MARKER_UPDATED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "update marker marker_name:" << feedback->marker_name;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
        return;
    }
    else if(MARKER_REMOVED == feedback->client_id)
    {
        std::ostringstream sstm;
        sstm << "remove marker marker_name:" << feedback->marker_name;
        RVIZ_COMMON_LOG_INFO(sstm.str().c_str());
        return;
    }
}


void SnapshotPlanePanel::handleActiveStatusChanged(bool active) {
    if (active) {
        setEnabled(true);
        parentWidget()->show();
    } else {
        setEnabled(false);
        parentWidget()->hide();
    }
}

void SnapshotPlanePanel::addPlaneButtonClicked()
{
    treeModel->appendGroup();
}


void SnapshotPlanePanel::removePlaneButtonClicked()
{
    int l1Row = -1;
    int l2Row = -1;
    getCurrentIndex(l1Row, l2Row);
    if(l1Row != -1) {
        removeGroup(l1Row);
    }
}


void SnapshotPlanePanel::removeGroup(int l1Row)
{
    // 删除group中的marker
    std::list<InteractiveMarker>& markers = treeModel->getGroupItems(l1Row);
    for(auto& marker: markers) {
        markerThread->erase(marker.name);
    }

    // 删除group sign marker
    std::string signName = treeModel->getGroupSign(l1Row);
    markerThread->erase(signName);

    // 删除model
    treeModel->removeGroup(l1Row);
}


void SnapshotPlanePanel::removePointButtonClicked()
{
    int l1Row = -1;
    int l2Row = -1;
    getCurrentIndex(l1Row, l2Row);
    if(l1Row != -1 && l2Row != -1)
    {
        InteractiveMarker& marker = treeModel->getGroupItem(l1Row, l2Row);
        markerThread->erase(marker.name);

        treeModel->removeGroupItem(l1Row, l2Row);
    }
}


void SnapshotPlanePanel::getCurrentIndex(int& l1Row, int& l2Row)
{
    l1Row = -1;
    l2Row = -1;
    QModelIndex currentIndex = ui->treeView->selectionModel()->currentIndex();
    if(currentIndex.isValid()) {
        // 父节点索引
        QModelIndex parentIndex = currentIndex.parent();
        // 有父节点则说明本节点是二级节点
        if (parentIndex.isValid()) {
            l1Row = parentIndex.row();
            l2Row = currentIndex.row();
        }
        // 没有父节点则说明本节点是一级节点
        else {
            l1Row = currentIndex.row();
        }
    }
}


void SnapshotPlanePanel::handleMapPointSelected(QString nameSpace, int pclIndex, pcl::PointXYZINormal& pclPoint)
{
    if(nameSpace == markerThread->objectName()) {

        int l1Row = -1;
        int l2Row = -1;
        getCurrentIndex(l1Row, l2Row);
        if(l1Row != -1)
        {  
            auto pointNormal = PointNormal::fromPclPoint(pclPoint);
            addGroupItem(l1Row, l2Row, pointNormal);
        }
    }
}


void SnapshotPlanePanel::addGroupItem(int l1Row, int l2Row, PointNormal pointNormal)
{
    InteractiveMarker intMarker = InteractiveMarkerFactory::createPcdPoint(pointNormal);
    markerThread->insert(intMarker);
    auto markers = treeModel->getGroupItems(l1Row);
    if(l2Row == -1)
    {
        l2Row = markers.size();
    }
    treeModel->insertGroupItem(l1Row, l2Row, intMarker);
}


void SnapshotPlanePanel::handlerMarkersReorder(int l1Row, int l2Row, std::list<InteractiveMarker>& markers)
{
    auto it = markers.begin();
    std::advance(it, l2Row); // 移动到指定索引

    for ( ;it != markers.end();++it) {
        int markerShowRow = getMetadataItem(*it, PointGroupModel::L2_ROW, "-1").toInt() + 1;
        it->description = std::to_string(markerShowRow);
        markerThread->insert(*it);
    }

    std::string signName = treeModel->getGroupSign(l1Row);
    if(markers.size() < 3)
    {
        markerThread->erase(signName);
    } else {
        InteractiveMarker signMarker = InteractiveMarkerFactory::createPrism(signName, markers);
        markerThread->insert(signMarker);
    }
    
}



void SnapshotPlanePanel::handleProjectLoaded(QSharedPointer<QVariantMap> projectData)
{
    QVariant photoPlanesVar = (*projectData)["photoPlanes"];
    Planes planes = photoPlanesVar.value<Planes>();
    
    int groupIdx = 0;
    for (const auto& plane : planes.planes)
    {
        treeModel->appendGroup();
    
        int itemIdx = 0;
        for(auto point: plane.points)
        {
            addGroupItem(groupIdx, itemIdx, point);
            itemIdx++;
        }
        groupIdx++;
    }
}


void SnapshotPlanePanel::handleProjectSaved(QSharedPointer<QVariantMap> projectData)
{
    auto markersList = treeModel->getGroups();

    Planes planes;
    int groupIdx = 1;
    for (auto markersIt = markersList.begin(); markersIt != markersList.end(); markersIt++, ++groupIdx) 
    {
        Plane plane;
        plane.id = groupIdx;
        
        int itemIdx = 1;
        for (auto markerIt = markersIt->begin(); markerIt != markersIt->end(); markerIt++, ++itemIdx)
        {
            QString pointStr = getMetadataItem(*markerIt, "point_normal");
            PointNormal point = json::parse(pointStr.toStdString()).get<PointNormal>();
            point.id = itemIdx;
            plane.points.push_back(point);
        }
        planes.planes.push_back(plane);
    }

    (*projectData)["photoPlanes"] = QVariant::fromValue(planes);
    
}


void SnapshotPlanePanel::handleProjectClosed(QSharedPointer<QVariantMap> projectData)
{
    int lastRow = treeModel->rowCount() - 1;
    for(;lastRow >= 0; lastRow--)
    {
        removeGroup(lastRow);
    }
}


}

}



#include <pluginlib/class_list_macros.hpp>
PLUGINLIB_EXPORT_CLASS(rviz_custom_plugins::panels::SnapshotPlanePanel, rviz_common::Panel )
